
input_xyz = csvread('karasev_serrano_calib_sticks1.csv');
input_uv  = csvread('cal01_xypts.csv');
input_xyz2 = csvread('karasev_serrano_calib_sticks2.csv');
input_uv2  = csvread('cal02_xypts.csv');
input_xyz3 = csvread('karasev_serrano_calib_sticks3.csv');
input_uv3  = csvread('cal03_xypts.csv');

% center the image at (0,0,1,t) ray
input_uv(:,2) = 480-input_uv(:,2); % mirror the y-coordinates, down is increasing
input_uv = input_uv - repmat([320 240],size(input_uv,1),1);
input_uv2(:,2) = 480-input_uv2(:,2); % mirror the y-coordinates, down is increasing
input_uv2 = input_uv2 - repmat([320 240],size(input_uv2,1),1);
input_uv3(:,2) = 480-input_uv3(:,2); % mirror the y-coordinates, down is increasing
input_uv3 = input_uv3 - repmat([320 240],size(input_uv3,1),1);


figure(1); hold off;
plot3(input_xyz(:,1),input_xyz(:,2),input_xyz(:,3),'.g'); hold on;
plot3(input_xyz2(:,1),input_xyz2(:,2),input_xyz2(:,3),'.b'); 
plot3(input_xyz3(:,1),input_xyz3(:,2),input_xyz3(:,3),'.b'); 
hold off; title('original 3D points');


% re-flip for display so it looks like the image where y is flipped
figure(2); hold off;
plot(input_uv(:,1),-input_uv(:,2),'g.'); hold on;
plot(input_uv2(:,1),-input_uv2(:,2),'b.');
plot(input_uv3(:,1),-input_uv3(:,2),'r.');
hold off;  title('projection view of points');

input_xyz = [input_xyz ones(size(input_xyz,1),1)]';
input_uv = [input_uv ones(size(input_uv,1),1)]';
input_xyz2 = [input_xyz2 ones(size(input_xyz2,1),1)]';
input_uv2 = [input_uv2 ones(size(input_uv2,1),1)]';
input_xyz3 = [input_xyz3 ones(size(input_xyz3,1),1)]';
input_uv3 = [input_uv3 ones(size(input_uv3,1),1)]';

figure(3)
img = imread('Image005.bmp'); imagesc(img); title('calibration object photo 1');
figure(4)
img = imread('Image006.bmp'); imagesc(img); title('calibration object photo 2');
figure(5)
img = imread('Image007.bmp'); imagesc(img); title('calibration object photo 3');

% down increasing y is correct; this leads to right-handed coordinates
% system. x cross y = z -> INTO the image. If x increases to the right
% then y must increase going down.

% Photo 1
c1 = CalibrateCameraAffine(input_xyz,input_uv);
pi0_Affine = reshape( [c1; 1]',4,3)';
cam0 = [1;1;1];
[cc,R,T,K,camAt,camLookAt] = CalibrateCameraNonLinear( input_xyz, input_uv, 100, cam0);
disp('Linear Affine Estimate:'); pi0_Affine
disp('Nonlinear Projective Estimate:'); pi0_NL = K * [R T]
uvReProj = pi0_NL * input_xyz;
figure(2); hold on;
plot(uvReProj(1,:),-uvReProj(2,:),'go'); hold off;
CalibData1.T = T; CalibData1.R = R; CalibData1.K = K; CalibData1.PI = pi0_NL;
CalibData1.xyz = input_xyz; CalibData1.uv = input_uv;
CalibData1.camAt = camAt; CalibData1.camLookAt = camLookAt;

% Photo 2
c1 = CalibrateCameraAffine(input_xyz2,input_uv2);
pi0_Affine = reshape( [c1; 1]',4,3)';
cam0 = [1 1 1];
[cc,R,T,K,camAt,camLookAt] = CalibrateCameraNonLinear( input_xyz2, input_uv2 , 100, cam0);
disp('Linear Affine Estimate:'); pi0_Affine
disp('Nonlinear Projective Estimate:'); pi0_NL = K * [R T]
uvReProj = pi0_NL * input_xyz2;
figure(2); hold on;
plot(uvReProj(1,:),-uvReProj(2,:),'bo'); hold off;
CalibData2.T = T; CalibData2.R = R; CalibData2.K = K; CalibData2.PI = pi0_NL;
CalibData2.xyz = input_xyz2; CalibData2.uv = input_uv2;
CalibData2.camAt = camAt; CalibData2.camLookAt = camLookAt;

% Photo 3
c1 = CalibrateCameraAffine(input_xyz3,input_uv3);
pi0_Affine = reshape( [c1; 1]',4,3)';
cam0 = [1 1 1];
[cc,R,T,K,camAt,camLookAt] = CalibrateCameraNonLinear( input_xyz3, input_uv3 , 100, cam0);
disp('Linear Affine Estimate:'); pi0_Affine
disp('Nonlinear Projective Estimate:'); pi0_NL = K * [R T]
uvReProj = pi0_NL * input_xyz3;
figure(2); hold on;
plot(uvReProj(1,:),-uvReProj(2,:),'ro'); hold off;
CalibData3.T = T; CalibData3.R = R; CalibData3.K = K; CalibData3.PI = pi0_NL;
CalibData3.xyz = input_xyz3; CalibData3.uv = input_uv3;
CalibData3.camAt = camAt; CalibData3.camLookAt = camLookAt;
